/*
 * @Author: MainStart pxclihai@126.com
 * @Date: 2024-07-24 15:19:34
 * @LastEditors: MainStart pxclihai@126.com
 * @LastEditTime: 2024-09-04 09:24:41
 * @FilePath: \nuohua_eided:\wdjg\project\wzjgwkfty03_mcu\babyos\bos\drivers\b_drv_dcMotor.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置:
 * https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
/**
 *!
 * \file
 * \version
 * \date
 * \author
 *******************************************************************************
 * @attention
 */

/*Includes ----------------------------------------------*/
#include "drivers/inc/b_drv_dcMotor.h"

#include "b_os.h"
#include "ddl.h"
#include "qp_port.h"
/**
 * \addtogroup B_DRIVER
 * \{
 */

/**
 * \addtogroup DCMOTOR
 * \{
 */

/**
 * \defgroup DCMOTOR_Private_TypesDefinitions
 * \{
 */

/**
 * \}
 */

/**
 * \defgroup DCMOTOR_Private_Defines
 * \{
 */
#define DRIVER_NAME DCMOTOR

// 诺华温控阀结构限位在 17826
#define MOTOR_MAX_LIMIT 14000  // 推杆转动长度
// 25BYJ412

// 定义相关阈值
#define STALL_CURRENT_THRESHOLD 300    // 800  // 堵转电流阈值，单位为mV
#define NO_LOAD_CURRENT_THRESHOLD 200  // 不带负载电流阈值，单位为mV
#define STARTUP_DELAY 2000  // 启动延迟时间，单位为ms//如果在校准的时候直接装上，并且
#define FILTER_SIZE 100  // 滤波窗口大小

// Define motor states
#define MOTOR_STOP 0
#define MOTOR_FWD 1
#define MOTOR_REV 2
#define MOTOR_STALL_FWD 3
#define MOTOR_STALL_REV 4
#define MOTOR_UNLOADED 5
#define MOTOR_WAIT_RUN 6

static uint8_t motor_state = MOTOR_STOP;
static uint8_t step_index  = 0;
// Target position
static int32_t target_position = 0;

// Current position
static int32_t current_position = 0;
// Callback function when motor run over
static drv_motor_run_over_cb_t motor_run_over_cb = NULL;

static bDriverInterface_t *_pdrv;

static uint8_t       is_stall_check             = 0;
static uint8_t       is_unloaded_check          = 0;
static uint8_t       stall_count                = 0;
static unsigned long startTime                  = 0;
static int           currentBuffer[FILTER_SIZE] = {0};
static int           currentIndex               = 0;
static uint32_t      currentTime                = 0;
static void          _motor_set_stall_check(uint8_t is_check);
static void          _motor_set_rev_limit(int limit);
static void          _motor_set_fwd_limit(int limit);
static void          _motor_set_trun_step(int32_t position, drv_cb_t cb);
/**
 * \}
 */

/**
 * \defgroup DCMOTOR_Private_Macros
 * \{
 */

/**
 * \}
 */

/**
 * \defgroup DCMOTOR_Private_Variables
 * \{
 */
/*
单4拍线圈上电时序表如下：
电极 1 2 3 4
    A+ 1 0 0 0
    A- 0 1 0 0
    B+ 0 0 1 0
    B- 0 0 0 1
    电机驱动采用2个H桥芯片，因此当输出2个高电平的时候为Break,立马刹车，输出为0
*/

static const uint8_t step_sequence[4][4] = {
    {1, 0, 1, 1},  // Step 1
    {0, 1, 1, 1},  // Step 3
    {1, 1, 1, 0},  // Step 1
    {1, 1, 0, 1},  // Step 3
};
// 电机8拍线圈上电时序表如下：
// static const uint8_t step_sequence[8][4] = {
//	{1, 0, 0, 0}, // Step 1
//	{1, 1, 0, 0}, // Step 2
//	{0, 1, 0, 0}, // Step 3
//	{0, 1, 1, 0}, // Step 4
//	{0, 0, 1, 0}, // Step 1
//	{0, 0, 1, 1}, // Step 2
//	{0, 0, 0, 1}, // Step 3
//	{1, 0, 0, 1}  // Step 4
// };
//  Current step index
bDRIVER_HALIF_TABLE(bDCMOTOR_HalIf_t, DRIVER_NAME);
/**
 * \}
 */

/**
 * \defgroup DCMOTOR_Private_FunctionPrototypes
 * \{
 */

/**
 * \}
 */

/**
 * \defgroup DCMOTOR_Private_Functions
 * \{
 */

#include "b_os.h"
void _set_motor_pins(bDriverInterface_t *_pdrv, uint8_t step)
{
    bDRIVER_GET_HALIF(_if, bDCMOTOR_HalIf_t, _pdrv);

    bMcuGpioWritePin(_if->INA1.port, _if->INA1.pin, step_sequence[step][0]);
    bMcuGpioWritePin(_if->INB1.port, _if->INB1.pin, step_sequence[step][1]);
    bMcuGpioWritePin(_if->INA2.port, _if->INA2.pin, step_sequence[step][2]);
    bMcuGpioWritePin(_if->INB2.port, _if->INB2.pin, step_sequence[step][3]);
}
static void _motor_set_rev_limit(int limit)
{
    bDRIVER_GET_HALIF(_if, bDCMOTOR_HalIf_t, _pdrv);
    _if->rev_limit_position = limit;
}
static void _motor_set_fwd_limit(int limit)
{
    bDRIVER_GET_HALIF(_if, bDCMOTOR_HalIf_t, _pdrv);
    _if->fwd_limit_position = limit;
}
static void _motor_turn_fwd(bDriverInterface_t *_pdrv)
{
    bDRIVER_GET_HALIF(_if, bDCMOTOR_HalIf_t, _pdrv);
    bMcuGpioWritePin(_if->INB1.port, _if->INB1.pin, 0);
    bMcuGpioWritePin(_if->INB2.port, _if->INB2.pin, 1);
}

static void _motor_turn_rev(bDriverInterface_t *_pdrv)
{
    bDRIVER_GET_HALIF(_if, bDCMOTOR_HalIf_t, _pdrv);
    bMcuGpioWritePin(_if->INB1.port, _if->INB1.pin, 1);
    bMcuGpioWritePin(_if->INB2.port, _if->INB2.pin, 0);
}
static void _motor_stop(bDriverInterface_t *_pdrv)
{
    bDRIVER_GET_HALIF(_if, bDCMOTOR_HalIf_t, _pdrv);
    bMcuGpioWritePin(_if->INA1.port, _if->INA1.pin, 1);
    bMcuGpioWritePin(_if->INB1.port, _if->INB1.pin, 1);
    bMcuGpioWritePin(_if->INA2.port, _if->INA2.pin, 1);
    bMcuGpioWritePin(_if->INB2.port, _if->INB2.pin, 1);
}
static void _motor_current_check_reset()
{
    stall_count  = 0;
    currentIndex = 0;
    currentTime  = 0;
}
static void _motor_reset_position(void)
{
    target_position  = 0;
    current_position = 0;
}
/*注意的是，去除堵转检测，那么电机运动将不会取判断极限位置，只判断位置步数*/
static void _motor_set_stall_check(uint8_t is_check)
{
    // b_log("is_stall_check:%d\n", is_check);
    // QF_CRIT_ENTRY();
    is_stall_check = is_check;
    //  QF_CRIT_EXIT();
}
uint16_t _motor_get_position_percent(void)
{

    uint16_t ret;
    bDRIVER_GET_HALIF(_if, bDCMOTOR_HalIf_t, _pdrv);
    // 确保极限位置是有效的，且正向极限位置大于反向极限位置
    if (_if->fwd_limit_position <= _if->rev_limit_position)
    {
        ret = 0;
        goto _exit;
    }
    // 计算整个可移动范围的长度
    int16_t range = _if->fwd_limit_position - _if->rev_limit_position;
    if (current_position <= _if->rev_limit_position)
    {
        ret = 0;
        goto _exit;
    }
    else if (current_position >= _if->fwd_limit_position)
    {
        ret = 1000;  // 100% 用1000表示
        goto _exit;
    }
    else
    {
        // 计算百分比（注意这里使用整数除法，所以乘以1000后再除以避免浮点运算）
        uint16_t percent =
            (uint32_t)(((current_position - _if->rev_limit_position) * 1000) / range);
        ret = percent;
        goto _exit;
    }
_exit:
    return ret;
}

static void _motor_set_trun_step(int32_t position, drv_motor_run_over_cb_t cb)
{
    target_position   = position;
    motor_run_over_cb = cb;
    // QF_CRIT_ENTRY();
    if (target_position > current_position)
    {
        motor_state = MOTOR_FWD;
    }
    else if (target_position < current_position)
    {
        motor_state = MOTOR_REV;
    }
    else
    {
        motor_state = MOTOR_STOP;
    }
    _motor_current_check_reset();  // warn 复位启动电流检测
    // QF_CRIT_EXIT();
    b_log("From pos %d to target pos %d\n", current_position, target_position);
}

static void _motor_set_position_precent(uint16_t precent, drv_motor_run_over_cb_t cb)
{
    uint8_t ret = 0;
    bDRIVER_GET_HALIF(_if, bDCMOTOR_HalIf_t, _pdrv);

    if (precent <= 0)
    {
        precent = 0;
    }
    if (precent > 1000)
    {
        precent = 1000;
    }
    if (_motor_get_position_percent() == precent)
    {
        ret       = 1;
        uint8_t v = 2;
        cb(_pdrv->drv_no, &v);

        goto _exit;
    }

    target_position = _if->rev_limit_position +
                      (_if->fwd_limit_position - _if->rev_limit_position) * (precent / 1000.0);
    b_log("rev_limit %d fwd_limit %d\n", _if->rev_limit_position, _if->fwd_limit_position);
_exit:
    if (ret == 0)
    {
        _motor_set_trun_step(target_position, cb);
        // b_log("target %d cur %d precent %d \n", target_position, current_position, precent);
    }
    if (ret == 1)
    {
        // b_log(
        //     "already in the current postiion cur precent: %d set precent: %d fwd limit: %d recv "
        //     "limit:%d\n",
        //     _motor_get_position_percent(), precent, _if->fwd_limit_position,
        //     _if->rev_limit_position);
    }
    return;
}
// 简单的滑动平均滤波器
static int filter(int *buffer, int size)
{
    int sum = 0;
    for (int i = 0; i < size; i++)
    {
        sum += buffer[i];
    }
    return sum / size;
}
// 电机堵转检测，需要放入adc中断中快速check
uint8_t drv_motor_check_isr(uint32_t current)
{
    // QF_CRIT_ENTRY();
    uint8_t ret;
    if (is_stall_check == 0)
    {
        ret = 1;
        goto _exit;
    }

    if (motor_state != MOTOR_FWD && motor_state != MOTOR_REV)
    {
        ret = 1;
        goto _exit;
    }

    // 检查是否在启动延迟时间内
    if (currentTime > STARTUP_DELAY)
    {
        // 滤波后的电流
        //	printf("time %d %d\n",currentTime,current);
        currentBuffer[currentIndex] = current;
        currentIndex                = (currentIndex + 1) % FILTER_SIZE;
        //		printf(" %d ",current);
        if (currentIndex == (FILTER_SIZE - 1))
        {
            int filteredCurrent = filter(currentBuffer, FILTER_SIZE);
            if (filteredCurrent > STALL_CURRENT_THRESHOLD)
            {
                _motor_stop(_pdrv);
                if (motor_state == MOTOR_FWD)
                {
                    motor_state = MOTOR_STALL_FWD;
                }
                else
                {
                    motor_state = MOTOR_STALL_REV;
                }
                b_log("stall current %d \n", filteredCurrent);
                ret = 0;
                goto _exit;
            }
            else if (filteredCurrent < NO_LOAD_CURRENT_THRESHOLD)
            {
                ret = 0;
                goto _exit;
            }
            else
            {
                ret = 0;
                goto _exit;
            }
        }
    }
_exit:
    // QF_CRIT_EXIT();
    return ret;
}
// 此函数必需放入精准的定时中运行。如2ms定时器
void drv_motor_process_isr(void)
{
    uint8_t ret = 0;
    bDRIVER_GET_HALIF(_if, bDCMOTOR_HalIf_t, _pdrv);
    // QF_CRIT_ENTRY();
    switch (motor_state)
    {
        case MOTOR_STOP:
            _motor_stop(_pdrv);
            break;
        case MOTOR_FWD:
            currentTime++;
            _motor_turn_fwd(_pdrv);
            current_position++;
            if (current_position == target_position)
            {
                motor_state = MOTOR_STOP;
                if (motor_run_over_cb != NULL)
                {
                    b_log("motor_run_over_cb %x\n", motor_run_over_cb);
                    ret = 0;
                    motor_run_over_cb(_pdrv->drv_no, &ret);
                    motor_run_over_cb = NULL;
                }
            }

            break;
        case MOTOR_REV:
            currentTime++;
            _motor_turn_rev(_pdrv);
            current_position--;
            if (current_position == target_position)
            {
                motor_state = MOTOR_STOP;
                if (motor_run_over_cb != NULL)
                {
                    ret = 0;
                    b_log("motor_run_over_cb %x\n", motor_run_over_cb);
                    motor_run_over_cb(_pdrv->drv_no, &ret);
                    motor_run_over_cb = NULL;
                }
            }

            break;
        case MOTOR_STALL_FWD:
            b_log("[motor fwd stall pos %d ]\n", current_position);
            //_if->fwd_limit_position = current_position + 1000;
            motor_state = MOTOR_WAIT_RUN;
            _motor_current_check_reset();
            ret = 1;
            if (motor_run_over_cb != NULL)
            {
                motor_run_over_cb(_pdrv->drv_no, &ret);
                motor_run_over_cb = NULL;
            }
            break;
        case MOTOR_STALL_REV:  
            b_log("[motor REV stall pos %d ]\n", current_position);
            current_position = 0;
            target_position  = 0;
            //_if->rev_limit_position = current_position + 100;
            motor_state = MOTOR_WAIT_RUN;
            _motor_current_check_reset();
            ret = 1;
            if (motor_run_over_cb != NULL)
            {
                motor_run_over_cb(_pdrv->drv_no, &ret);
                motor_run_over_cb = NULL;
            }
            break;
        case MOTOR_UNLOADED:
            // b_log("[motor unloaded stall]\n");
            motor_state = MOTOR_WAIT_RUN;
            break;
        case MOTOR_WAIT_RUN:
            _motor_current_check_reset();
            motor_state = MOTOR_STOP;
            break;
    }
    // QF_CRIT_EXIT();
}

static int _bDCMOTORCtl(bDriverInterface_t *pdrv, uint8_t cmd, void *param)
{
  //   b_log("set motor cmd %d \n", cmd);
    // QF_CRIT_ENTRY();
    if (cmd == bCMD_MOTOR_SET_STALL_CHECK)
    {
        _motor_set_stall_check(*(uint8_t *)param);
    }
    else if (cmd == bCMD_MOTOR_TURN_REV_LIMIT)
    {
        _motor_set_stall_check(1);  // 打开堵转检测
        _motor_set_trun_step(-MOTOR_MAX_LIMIT * 2, (drv_motor_run_over_cb_t)param);
    }
    else if (cmd == bCMD_MOTOR_TURN_FWD_LIMIT)
    {
        _motor_set_stall_check(1);  // 打开堵转检测
        _motor_set_trun_step(MOTOR_MAX_LIMIT, (drv_motor_run_over_cb_t)param);
    }
    else if (cmd == bCMD_MOTOR_SET_POSITION_PERCENT)
    {
        _motor_set_stall_check(0);  // 打开堵转检测
        bSetMotor_t *p = (bSetMotor_t *)param;
        _motor_set_position_precent(p->percent, p->cb);
    }
    else if (cmd == bCMD_MOTOR_GET_POSITION_PERCENT)
    {
        *(uint16_t *)param = _motor_get_position_percent();
    }
    else if (cmd == bCMD_MOTOR_RESET_POSITION)
    {
        _motor_reset_position();
    }
    // QF_CRIT_EXIT();
}
static int _bStepMotorRead(bDriverInterface_t *pdrv, uint32_t off, uint8_t *pbuf, uint32_t len)
{
}
/**
 * \}
 */

/**
 * \addtogroup DCMOTOR_Exported_Functions
 * \{
 */
#include "b_os.h"
int bDCMOTOR_Init(bDriverInterface_t *pdrv)
{
    bDRIVER_STRUCT_INIT(pdrv, DRIVER_NAME, bDCMOTOR_Init);
    bDRIVER_GET_HALIF(_if, bDCMOTOR_HalIf_t, pdrv);
    _pdrv      = pdrv;
    pdrv->read = _bStepMotorRead;
    pdrv->ctl  = _bDCMOTORCtl;

    bMcuGpioConfig(_if->pwr_en.port, _if->pwr_en.pin, B_HAL_GPIO_OUTPUT, B_HAL_GPIO_PULLDOWN);
    bMcuGpioWritePin(_if->pwr_en.port, _if->pwr_en.pin, 0);
    bMcuGpioConfig(B_HAL_GPIOA, B_HAL_PIN11, B_HAL_GPIO_OUTPUT, B_HAL_GPIO_NOPULL);  // open power
    bMcuGpioWritePin(B_HAL_GPIOA, B_HAL_PIN11, 1);

    bMcuGpioConfig(_if->INA1.port, _if->INA1.pin, B_HAL_GPIO_OUTPUT, B_HAL_GPIO_NOPULL);
    bMcuGpioConfig(_if->INA2.port, _if->INA2.pin, B_HAL_GPIO_OUTPUT, B_HAL_GPIO_NOPULL);
    bMcuGpioConfig(_if->INB1.port, _if->INB1.pin, B_HAL_GPIO_OUTPUT, B_HAL_GPIO_NOPULL);
    bMcuGpioConfig(_if->INB2.port, _if->INB2.pin, B_HAL_GPIO_OUTPUT, B_HAL_GPIO_NOPULL);
    is_stall_check = 1;
    return 0;
}

#ifdef BSECTION_NEED_PRAGMA
#pragma section driver_init
#endif
bDRIVER_REG_INIT(B_DRIVER_DCMOTOR, bDCMOTOR_Init);
#ifdef BSECTION_NEED_PRAGMA
#pragma section
#endif
/*
 * @brief
 */
static bool motor_is_idle(void)
{
    return motor_state == MOTOR_STOP ? true : false;
}

bPM_REG_INSTANCE("pm_motor", motor_is_idle, NULL, NULL);
/**
 * \}
 */

/**
 * \}
 */

/************************ Copyright (c) 2020 Bean *****END OF FILE****/
